%% Save breakpoints and clear all

s=dbstatus('-completenames');
save bkpt s

%% clear
clear classes; close all hidden; clc;
%% Set up paths (Java)
setSimPaths;
%% restore breakpoitns
load bkpt
dbstop(s);
%%

%Satellite structure
global SPH c test_number

% Load generic global variables
globalvarSPHERES;

%% Configuration Parameters
% Use this section to configure parameters related to the simulation
% environment and execution options

test_number = 1;
engTrace = 0;
fastmode = 0;
gspMode = 'c';
simTimeout = 30000; % Simulation timeout in ms
testLocation = c.LOCATION_ISS;
animateOn = 1; %Turns the animation on or off

%% Satellite configuration
% Use this section to initialize the satellites in the simulation


%Initialize SPHERES
Sphere1_SN = 2;
ctrlState_Sphere1 = [0;  0.3; 0;  0; 0; 0;...
                      1;  0;  0;  0;  0; 0; 0];
SPH{1} = Sphere(testLocation, Sphere1_SN, ctrlState_Sphere1, 1);

Sphere2_SN = 3;
ctrlState_Sphere2 = zeros(13,1);
ctrlState_Sphere2(7) = 1.0;
SPH{2} = Sphere(testLocation, Sphere2_SN, ctrlState_Sphere2, 2);

Sphere3_SN = 4;
ctrlState_Sphere3 = zeros(13,1);
ctrlState_Sphere3(4) = 1.0;
SPH{3} = Sphere(testLocation, Sphere3_SN, ctrlState_Sphere3, 3);

%% Cleanup 
clear ctrlState_Sphere1 Sphere1_SN Sphere2_SN ctrlState_Sphere2

%% Run the simulation
mainScheduler;

% data conversion;
data = convertDebugData(SPH);

%% Save results
for i = 1:length(SPH)
    SPH{i}.makeSaveable;
end
clear eng;
save sim_results.mat SPH data
